#include "fgen.h"

using namespace physics;

//////////////////////////////////////////////
//					Gravity
//////////////////////////////////////////////
Gravity::Gravity(const Vector3& gravity)
	: gravity(gravity)
{
}

void Gravity::updateForce(RigidBody* body, real dt)
{
	// check if we have a finite mass
	if (!body->hasFiniteMass()) 
		return;

	// apply a mass-scaled force to the body
	if (body->getAwake())
		body->applyForce(gravity * body->getMass());
}

//////////////////////////////////////////////
//					Explosion
//////////////////////////////////////////////
/*Explosion::Explosion(const Vector3 &detonation,
					const real &speed,
					const real &thickness,
					const real &peakForce,
					const real &duration)
	:	detonation(detonation),
		shockwaveSpeed(speed),
		shockwaveThickness(thickness),
		peakConcussionForce(peakForce),
		concussionDuration(duration)

{
}*/

Explosion::Explosion(const Vector3 &detonation,
					const real &radius,
					const real &peakForce)
	: detonation(detonation),
		radius(radius),
		peakConcussionForce(peakForce)
{
}

void Explosion::updateForce(RigidBody* body, real dt)
{
	if (!body->getActivated())
		return;

	Vector3 pos;
	body->getPosition(&pos);
	Vector3 distance = pos - detonation;
	real magSqr = distance.squareMagnitude();
	if (magSqr > radius * radius)
		return;

	Vector3 force = distance;
	force.normalize();
	
	force *= peakConcussionForce / magSqr;
	float mag = force.magnitude();
	body->applyForce(force * (1 / dt));
}

//////////////////////////////////////////////
//				ForceRegistry
//////////////////////////////////////////////

//////////////////////////////////////////////
// add()
//
// adds the passed object and FG (force generator) to
// the list of force generators
//////////////////////////////////////////////
void ForceRegistry::add(RigidBody *object, ForceGenerator *fg)
{
	ForceRegistration r;
	r.object = object;
	r.fg = fg;
	bool found = false;
	Registry::iterator i = forceGenerators.begin();

	// check if the object/fg combo already exists
	while(i != forceGenerators.end())
	{
		if ((*i).object == object && (*i).fg == fg)
		{
			found = true;
			break;
		}
		
		i++;
	}

	if (!found)
		forceGenerators.push_back(r);
}

//////////////////////////////////////////////
// remove()
//
// remove the passed object and FG (force generator) 
// from the list of force generators
//////////////////////////////////////////////
void ForceRegistry::remove(RigidBody *object)
{
	Registry::iterator i = forceGenerators.begin();

	// check if the object/fg combo already exists
	while(i != forceGenerators.end())
	{
		if ((*i).object == object)
			i = forceGenerators.erase(i);
		else
			i++;
	}
}



//////////////////////////////////////////////
// updateForces()
//
// Tells all stored Object/FG combinations to update
//////////////////////////////////////////////
void ForceRegistry::updateForces(real dt)
{
	// loop through all FGs telling them to update their objects
	Registry::iterator i = forceGenerators.begin();
	while (i != forceGenerators.end())
	{
		// what updateForce() does, depends on the
		// overloaded function of the child-FG (recall that
		// the FG class is an abstract base class)
		i->fg->updateForce(i->object, dt);
		i++;
	}
}


//////////////////////////////////////////////
//			GlobalForceRegistry
//////////////////////////////////////////////

//////////////////////////////////////////////
// add()
//
// adds the passed FG (force generator) to
// the list of force generators
//////////////////////////////////////////////
void GlobalForceRegistry::add(ForceGenerator *fg)
{
	bool found = false;
	GlobalRegistry::iterator i = forceGenerators.begin();

	// check if the fg already exists
	while(i != forceGenerators.end())
	{
		if ((*i) == fg)
		{
			found = true;
			break;
		}
		
		i++;
	}

	if (!found)
		forceGenerators.push_back(fg);
}

//////////////////////////////////////////////
// remove()
//
// remove the passed FG (force generator) 
// from the list of force generators
//////////////////////////////////////////////
void GlobalForceRegistry::remove(ForceGenerator *fg)
{
	GlobalRegistry::iterator i = forceGenerators.begin();

	// check if the object/fg combo already exists
	while(i != forceGenerators.end())
	{
		if ((*i) == fg)
			i = forceGenerators.erase(i);
		else
			i++;
	}
}



//////////////////////////////////////////////
// updateForces()
//
// Applise all global FGs to update all objects
//////////////////////////////////////////////
void GlobalForceRegistry::updateForces(real dt)
{
	// loop through all FGs telling them to update their objects
	GlobalRegistry::iterator i = forceGenerators.begin();
	Objects::iterator j;
	while (i != forceGenerators.end())
	{
		j = pObjectList->begin();
		while (j != pObjectList->end())
		{
			if ((*j)->body != NULL)
				if ((*j)->body->getActivated()) // deactivated objects are not subject to global forces
					(*i)->updateForce((*j)->body, dt);
			j++;
		}
		i++;		
	}
}


//////////////////////////////////////////////
// Drag
//////////////////////////////////////////////
//////////////////////////////////////////////
// Drag()
// 
// Creates the drag force based on the two components
// passed. k1 and k2 represent the first and second-order
// coefficients for the drag force (which depends on velocity)
//////////////////////////////////////////////
Drag::Drag(real k1, real k2)
: k1(k1), k2(k2)
{
}

//////////////////////////////////////////////
// updateForce()
//
// Applies the drag force to all Particles
//////////////////////////////////////////////
void Drag::updateForce(RigidBody* object, real dt)
{
	// get the current velocity (call it force
	// since we'll apply this Vector as a force later)
	Vector3 force;
	object->getVelocity(&force);

	// Calculate the total drag coefficient
	real dragCoeff = force.magnitude();
	dragCoeff = k1 * dragCoeff + k2 * dragCoeff * dragCoeff;

	// Calculate the final force and apply it
	// keep the same direction, but multiply it by the
	// total drag
	force.normalize();
	force *= -dragCoeff;
	object->applyForce(force);
}